
#include <mutex.h>
#include <syscall.h>
#include <Thread.h>
#include "i8042.h"
#include "libps2.h"

#if 0
#define dbg printf
#else
#define dbg(fmt, ...)
#endif

#define task_getTid() syscall(SYSCALL_THREAD, SYSCALL_THREAD_GET_TID)
#define task_sleep(m) syscall(SYSCALL_THREAD, SYSCALL_THREAD_SLEEP, (m))
#define task_notify(tid) syscall(SYSCALL_THREAD, SYSCALL_THREAD_NOTIFY, (tid))

#define __wait_event_timeout(wq, condition, ret)	\
do {												\
	mutex_wait(&wq.mutex);							\
	wq.tid = task_getTid();							\
	if (!(condition))								\
		task_sleep(ret);							\
	wq.tid = 0;										\
	mutex_signal(&wq.mutex);						\
} while (0)

#define wait_event_timeout(wq, condition, timeout)	\
({													\
	long __ret = timeout;							\
	if (!(condition)) 								\
		__wait_event_timeout(wq, condition, __ret);	\
	__ret;											\
})

#define wake_up(wq)									\
do {												\
	if (wq.tid)										\
		task_notify(wq.tid);						\
} while (0)

#define msecs_to_jiffies(a) a

/*
 * ps2_sendbyte() sends a byte to the mouse, and waits for acknowledge.
 * It doesn't handle retransmission, though it could - because when there would
 * be need for retransmissions, the mouse has to be replaced anyway.
 *
 * ps2_sendbyte() can only be called from a process context
 */

int ps2_sendbyte(struct ps2dev *ps2dev, unsigned char byte, int timeout)
{
	dbg("ps2_sendbyte: enter\n");
	
	//serio_pause_rx(ps2dev->serio);
	interrupt_mutex_wait(&ps2dev->serio);
	ps2dev->nak = 1;
	ps2dev->flags |= PS2_FLAG_ACK;
	interrupt_mutex_signal(&ps2dev->serio);
	//serio_continue_rx(ps2dev->serio);

	if (i8042_aux_write(byte) == 0)
		wait_event_timeout(ps2dev->wait, !(ps2dev->flags & PS2_FLAG_ACK),
				   msecs_to_jiffies(timeout));

	//serio_pause_rx(ps2dev->serio);
	interrupt_mutex_wait(&ps2dev->serio);
	ps2dev->flags &= ~PS2_FLAG_ACK;
	interrupt_mutex_signal(&ps2dev->serio);
	//serio_continue_rx(ps2dev->serio);
	
	dbg("ps2_sendbyte: exit\n");
	
	return -ps2dev->nak;
}

/*
 * ps2_command() sends a command and its parameters to the mouse,
 * then waits for the response and puts it in the param array.
 *
 * ps2_command() can only be called from a process context
 */

int ps2_command(struct ps2dev *ps2dev, unsigned char *param, int command)
{
	dbg("ps2_command: enter\n");
	
	int timeout;
	int send = (command >> 12) & 0xf;
	int receive = (command >> 8) & 0xf;
	int rc = -1;
	int i;

	mutex_wait(&ps2dev->cmd_sem);

	//serio_pause_rx(ps2dev->serio);
	interrupt_mutex_wait(&ps2dev->serio);
	ps2dev->flags = command == PS2_CMD_GETID ? PS2_FLAG_WAITID : 0;
	ps2dev->cmdcnt = receive;
	if (receive && param)
		for (i = 0; i < receive; i++)
			ps2dev->cmdbuf[(receive - 1) - i] = param[i];
	interrupt_mutex_signal(&ps2dev->serio);
	//serio_continue_rx(ps2dev->serio);

	/*
	 * Some devices (Synaptics) peform the reset before
	 * ACKing the reset command, and so it can take a long
	 * time before the ACK arrrives.
	 */
	if (command & 0xff)
		if (ps2_sendbyte(ps2dev, command & 0xff,
			command == PS2_CMD_RESET_BAT ? 1000 : 200))
			goto out;

	for (i = 0; i < send; i++)
		if (ps2_sendbyte(ps2dev, param[i], 200))
			goto out;

	/*
	 * The reset command takes a long time to execute.
	 */
	timeout = msecs_to_jiffies(command == PS2_CMD_RESET_BAT ? 4000 : 500);

	timeout = wait_event_timeout(ps2dev->wait,
				     !(ps2dev->flags & PS2_FLAG_CMD1), timeout);

	if (ps2dev->cmdcnt && timeout > 0) {

		if (command == PS2_CMD_RESET_BAT && timeout > msecs_to_jiffies(100)) {
			/*
			 * Device has sent the first response byte
			 * after a reset command, reset is thus done,
			 * shorten the timeout. The next byte will come
			 * soon (keyboard) or not at all (mouse).
			 */
			timeout = msecs_to_jiffies(100);
		}

		if (command == PS2_CMD_GETID &&
		    ps2dev->cmdbuf[receive - 1] != 0xab && /* Regular keyboards */
		    ps2dev->cmdbuf[receive - 1] != 0xac && /* NCD Sun keyboard */
		    ps2dev->cmdbuf[receive - 1] != 0x2b && /* Trust keyboard, translated */
		    ps2dev->cmdbuf[receive - 1] != 0x5d && /* Trust keyboard */
		    ps2dev->cmdbuf[receive - 1] != 0x60 && /* NMB SGI keyboard, translated */
		    ps2dev->cmdbuf[receive - 1] != 0x47) { /* NMB SGI keyboard */
			/*
			 * Device behind the port is not a keyboard
			 * so we don't need to wait for the 2nd byte
			 * of ID response.
			 */
			//serio_pause_rx(ps2dev->serio);
			interrupt_mutex_wait(&ps2dev->serio);
			ps2dev->flags = ps2dev->cmdcnt = 0;
			interrupt_mutex_signal(&ps2dev->serio);
			//serio_continue_rx(ps2dev->serio);
		}

		wait_event_timeout(ps2dev->wait,
				   !(ps2dev->flags & PS2_FLAG_CMD), timeout);
	}

	if (param)
		for (i = 0; i < receive; i++)
			param[i] = ps2dev->cmdbuf[(receive - 1) - i];

	if (ps2dev->cmdcnt && (command != PS2_CMD_RESET_BAT || ps2dev->cmdcnt != 1))
		goto out;

	rc = 0;

out:
	//serio_pause_rx(ps2dev->serio);
	interrupt_mutex_wait(&ps2dev->serio);
	ps2dev->flags = 0;
	interrupt_mutex_signal(&ps2dev->serio);
	//serio_continue_rx(ps2dev->serio);
	
	mutex_signal(&ps2dev->cmd_sem);
	dbg("ps2_command: exit\n");
	return rc;
}

/*
 * ps2_init() initializes ps2dev structure
 */

void ps2_init(struct ps2dev *ps2dev)
{
	mutex_init(&ps2dev->cmd_sem);
	mutex_init(&ps2dev->wait.mutex);
	interrupt_mutex_init(&ps2dev->serio);
	ps2dev->wait.tid = 0;
	//ps2dev->serio = serio;
}

/*
 * ps2_handle_ack() is supposed to be used in interrupt handler
 * to properly process ACK/NAK of a command from a PS/2 device.
 */

int ps2_handle_ack(struct ps2dev *ps2dev, unsigned char data)
{
	switch (data) {
		case PS2_RET_ACK:
			ps2dev->nak = 0;
			break;

		case PS2_RET_NAK:
			ps2dev->nak = 1;
			break;

		/*
		 * Workaround for mice which don't ACK the Get ID command.
		 * These are valid mouse IDs that we recognize.
		 */
		case 0x00:
		case 0x03:
		case 0x04:
			if (ps2dev->flags & PS2_FLAG_WAITID) {
				ps2dev->nak = 0;
				break;
			}
			/* Fall through */
		default:
			return 0;
	}


	if (!ps2dev->nak && ps2dev->cmdcnt)
		ps2dev->flags |= PS2_FLAG_CMD | PS2_FLAG_CMD1;

	ps2dev->flags &= ~PS2_FLAG_ACK;
	wake_up(ps2dev->wait);

	if (data != PS2_RET_ACK)
		ps2_handle_response(ps2dev, data);

	return 1;
}

/*
 * ps2_handle_response() is supposed to be used in interrupt handler
 * to properly store device's response to a command and notify process
 * waiting for completion of the command.
 */

int ps2_handle_response(struct ps2dev *ps2dev, unsigned char data)
{
	if (ps2dev->cmdcnt)
		ps2dev->cmdbuf[--ps2dev->cmdcnt] = data;

	if (ps2dev->flags & PS2_FLAG_CMD1) {
		ps2dev->flags &= ~PS2_FLAG_CMD1;
		if (ps2dev->cmdcnt)
			wake_up(ps2dev->wait);
	}

	if (!ps2dev->cmdcnt) {
		ps2dev->flags &= ~PS2_FLAG_CMD;
		wake_up(ps2dev->wait);
	}

	return 1;
}

void ps2_cmd_aborted(struct ps2dev *ps2dev)
{
	if (ps2dev->flags & PS2_FLAG_ACK)
		ps2dev->nak = 1;

	if (ps2dev->flags & (PS2_FLAG_ACK | PS2_FLAG_CMD))
		wake_up(ps2dev->wait);

	ps2dev->flags = 0;
}

